#include <sstream>
#include <vector>
#include "robot_manager.h"

#include "utilities/convert_data_type.hpp"
#include "utilities/file.hpp"
#include "utilities_function.h"

void RobotManager::commandMoveTrajCallback(const std_msgs::msg::String::SharedPtr msg)
{
    CmdMsg msg_;
    msg_.str = std::move(msg);
    {
        std::unique_lock<std::mutex> lock(mutex_);
        move_msg_queue_.emplace(CmdType::MOVE_TRAJ, msg_);
    }
    cv_.notify_one();
}

void RobotManager::moveTrajFunc(const CmdMsg &msg)
{
    std::istringstream ss(msg.str->data);
    ZLOGI("move traj rec data: %s", ss.str().c_str());
    std::vector<std::vector<double>> traj = ReadData2D(ss.str(), ',');
    rclcpp::Rate rate(CONTROL_FREQ);
    int j = 0;
    int N = traj.size();
    while (j < N)
    {
        pthread_mutex_lock(&zoros_mutex_);
        std::vector<double> actual_position = joint_state_msg_.position;
        pthread_mutex_unlock(&zoros_mutex_);

        if (IsClose(actual_position, traj[N - 1], JOINT_CONTROL_TOLERANCE))
        {
            break;
        }

        pthread_mutex_lock(&zoros_mutex_);
        Eigen::VectorXd q, qd, qdd, torque;
        q = Vector2Eigen(traj[j]);
        qd.setZero(robot_dof_);
        qdd.setZero(robot_dof_);
        robot_algo_->id_->inverseDynamics(q, qd, qdd, torque);
        ZLOGI("index [%d]: %f, %f, %f, %f, %f, %f, %f", j, q[0], q[1], q[2], q[3], q[4], q[5], q[6]);
        for (size_t i = 0; i < 7; i++)
        {
            if (!is_simulation_)
            {
                //! 实机模式下还需调用 DM SDK 获取实际的关节角度
                dm_->control_mit(motors_[i], mit_kp_[i], mit_kd_[i], q[i], 0.0, torque[i] * torque_gain_[i]);
            }
            else
            {
                joint_state_msg_.position[i] = q[i];
                //! 如果是仿真模式，发布一条初始消息，所有关节的位置为0.0
                joint_state_msg_.header.stamp = this->now();
                joint_pub_->publish(joint_state_msg_); // 发布初始的 joint_states 消息
            }
        }
        pthread_mutex_unlock(&zoros_mutex_);
        j++;
        rate.sleep();
    }
    //     std::istringstream ss(msg->data);
    //     ZLOGI("move traj rec data: %s", ss.str().c_str());
    //
    //     std::vector<double> mit_kp, mit_kd;
    //     this->get_parameter("mit_kp", mit_kp);
    //     this->get_parameter("mit_kd", mit_kd);
    //
    // //    std::vector<std::vector<double>> traj;
    //      std::vector<std::vector<double>> traj = ReadData2D(ss.str(), ',');
    //
    //     for (size_t k = 0; k < traj.size(); k++)
    //     {
    //         ZLOGI("[%d]: %f, %f, %f, %f, %f, %f, %f", k, traj[k][0], traj[k][1], traj[k][2], traj[k][3], traj[k][4],
    //         traj[k][5], traj[k][6]);
    //     }
    //     ZLOGI("traj_pos_size: %ld", traj.size());
    //
    //     //! 线性插值过程
    //     rclcpp::Rate rate(CONTROL_FREQ);
    //     int k = 0;
    //     std::vector<double> q(7, 0.0), qd(7, 0.0), qdd(7, 0.0), torque;
    //     while (1)
    //     {
    //         if (k >= static_cast<int>(traj.size()))
    //         {
    //             break;
    //         }
    //
    //         pthread_mutex_lock(&zoros_mutex_);
    //         q = joint_state_msg_.position;
    //         robot_model_->inverseDynamics(q, qd, qdd, torque);
    //         for (size_t i = 0; i < 7; i++)
    //         {
    //             if (!is_simulation_)
    //             {
    //                 //! 实机模式下还需调用 DM SDK 获取实际的关节角度
    //                 // dm_->control_pos_vel(motors_[i], traj[k][i], 10.0);
    //                 dm_->control_mit(motors_[i], mit_kp[i], mit_kd[i], traj[k][i], 0.0, torque[i]);
    //             }
    //             else
    //             {
    //                 joint_state_msg_.position[i] = traj[k][i];
    //                 //! 如果是仿真模式，发布一条初始消息，所有关节的位置为0.0
    //                 joint_state_msg_.header.stamp = this->now();
    //                 joint_pub_->publish(joint_state_msg_); // 发布初始的 joint_states 消息
    //             }
    //         }
    //         pthread_mutex_unlock(&zoros_mutex_);
    //
    //         k++;
    //         rate.sleep();
    //     }
}
